/*
 * control.cpp
 *
 *  Created on: 2020年9月2日
 *      Author: 17900
 */
#include "control.hpp"

CONTROL control;

void CONTROL::Control_Init()
{
	CtrlIO.control_mode=0;
	CtrlIO.fly_mode=0;
	CtrlIO.yaw_mode=0;
	CtrlIO.gyro_gain=0.0f;
	CtrlIO.cs_output[4]=1200;
	CtrlIO.mid_trim[0]=0.0f;
	CtrlIO.mid_trim[1]=0.0f;
	CtrlIO.mid_trim[2]=0.0f;
	CtrlKiTemp.int_reset=true;
	CtrlFbck.engine_speed=0.0f;
	CtrlLpIO.position_mode=false;
	CtrlLpIO.brake_mode=false;
	CtrlLpIO.AngleLimitR=20*D2R;
	CtrlLpIO.AngleLimitP=20*D2R;

	//-----------------------------------------------by mch
	dir_alloc_mch_initialize();
	wls_alloc_mch_initialize();
	//----------------------------------------------------------
	CtrlIO.rc_cnt = 0;
	CtrlIO.rc_check = 0;
	CtrlIO.ever_rc = 0;
	CtrlIO.rc_status=NORMAL;

	CtrlLpIO.VelSet=0.0;
	CtrlLpIO.k1=100.0f;
	CtrlLpIO.ko1=2.0f;

	for(int i=0; i<RC_CHECK_NUM; i++)
	{
		CtrlIO.rc_buffer[i] = 0;
	}


	//pidRolRate
	pidRolRate->Kp = 0.4f;
	pidRolRate->Ki = 0.2f;
	pidRolRate->Kd = 0.0f;
	pidRolRate->Kb = 0.0f;
	pidRolRate->eLimit = 4*PI;
	pidRolRate->iLimit = 20*D2R;
	pidRolRate->dLimit = PI;

	//pidPitRate
	pidPitRate->Kp = 0.4f;
	pidPitRate->Ki = 0.2f;
	pidPitRate->Kd = 0.0f;
	pidPitRate->Kb = 0.0f;
	pidPitRate->eLimit = 4*PI;
	pidPitRate->iLimit = 20*D2R;
	pidPitRate->dLimit = PI;

	//pidYawRate
	pidYawRate->Kp = 0.24f;
	pidYawRate->Ki = 0.1f;
	pidYawRate->Kd = 0.0f;
	pidYawRate->Kb = 0.0f;
	pidYawRate->eLimit = 4*PI;
	pidYawRate->iLimit = 20*D2R;
	pidYawRate->dLimit = PI;

	//pidRol
	pidRol->Kp = 7.0f;
	pidRol->Ki = 0.0f;
	pidRol->Kd = 0.0f;
	pidRol->Kb = 0.0f;
	pidRol->eLimit = PI;
	pidRol->iLimit = PI;

	//pidPit
	pidPit->Kp = 7.0f;
	pidPit->Ki = 0.0f;
	pidPit->Kd = 0.0f;
	pidPit->Kb = 0.0f;
	pidPit->eLimit = PI;
	pidPit->iLimit = PI;

	//pidYaw
	pidYaw->Kp = 4.0f;
	pidYaw->Ki = 0.0f;
	pidYaw->Kd = 0.0f;
	pidYaw->Kb = 0.0f;
	pidYaw->eLimit = PI;
	pidYaw->iLimit = PI;

	//pidXRate
	pidXRate->Kp = 1.0f;
	pidXRate->Ki = 0.1f;
	pidXRate->Kd = 0.05f;
	pidXRate->Kb = 0.0f;
	pidXRate->eLimit = 20;
	pidXRate->iLimit = 20;
	pidXRate->dLimit = 20;

	//pidYRate
	pidYRate->Kp = 1.0f;
	pidYRate->Ki = 0.1f;
	pidYRate->Kd = 0.05f;
	pidYRate->Kb = 0.0f;
	pidYRate->eLimit = 20;
	pidYRate->iLimit = 20;
	pidYRate->dLimit = 20;

	//pidZRate
	pidZRate->Kp = 4.0f;
	pidZRate->Ki = 0.1f;
	pidZRate->Kd = 0.1f;
	pidZRate->Kb = 0.0f;
	pidZRate->eLimit = 20;
	pidZRate->iLimit = 200;
	pidZRate->dLimit = 20;

	//pidX
	pidX->Kp = 0.5f;
	pidX->Ki = 0.0f;
	pidX->Kd = 0.0f;
	pidX->Kb = 0.0f;
	pidX->eLimit = 200;
	pidX->iLimit = 200;
	pidX->dLimit = 200;

	//pidY
	pidY->Kp = 0.5f;
	pidY->Ki = 0.0f;
	pidY->Kd = 0.0f;
	pidY->Kb = 0.0f;
	pidY->eLimit = 200;
	pidY->iLimit = 200;
	pidY->dLimit = 200;

	//pidZ
	pidZ->Kp = 1.0f;
	pidZ->Ki = 0.01f;
	pidZ->Kd = 0.01f;
	pidZ->Kb = 0.0f;
	pidZ->eLimit = 20;
	pidZ->iLimit = 200;
	pidZ->dLimit = 20;

	//pidTmp
	pidTmp->Kp = 300.0f;
	pidTmp->Ki = 0.07f;
	pidTmp->Kd = 20.02f;
	pidTmp->filter_para = 0.557f;
	pidTmp->setpoint=40.0f;

	//pidThrust
	pidThrust->Kp = 0.0f;
	pidThrust->Ki = 0.02f;
	pidThrust->Kd = 0.0f;
	pidThrust->Kb = 0.0f;
	pidThrust->eLimit = 200;
	pidThrust->iLimit = 200;
	pidThrust->dLimit = 200;


	for(u8 i=0;i<PID_NUM;i++)
	{
		pid_msg.kp[i] = pid[i].Kp;
		pid_msg.ki[i] = pid[i].Ki;
		pid_msg.kd[i] = pid[i].Kd;
	}
	xQueueOverwrite(queuePID,&pid_msg);

}



void CONTROL::Control_Feedback()
{
	xQueuePeek(queueGyrDat, &gyro, 0);					//从队列中获取陀螺仪数据
	xQueuePeek(queueAhrsEuler, &ahrsEuler, 0);					//从队列中获取角度、旋转矩阵数据
	xQueuePeek(queueHeight, &height, 0);					//从队列中获取高度数据
	xQueuePeek(queueFlow, &flow, 0);					//从队列中获取高度数据

	CtrlFbck.AirSpeed_Last   = CtrlFbck.AirSpeed;



	CtrlFbck.X[0] = flow.flowx2;
	CtrlFbck.Y[0] = flow.flowy2;
	CtrlFbck.Z[0]= -height.height*0.01f;

	CtrlFbck.X[1] = flow.flowx*0.001f;
	CtrlFbck.Y[1] = flow.flowy*0.001f;
	CtrlFbck.Z[1]=  -height.speedZ*0.01f;

	CtrlFbck.XH[1] =  CtrlFbck.X[1];
	CtrlFbck.YH[1] =  CtrlFbck.Y[1];


	for (u8 i=0;i<3;i++)
	{
		CtrlFbck.pqr[i]=gyro.gyro[i];
		CtrlFbck.Ang[i]=ahrsEuler.Ang[i];
	}
	//----------北东地位置----------------------

	CtrlFbck.AirSpeed = sqrt(CtrlFbck.X[1]*CtrlFbck.X[1]+CtrlFbck.Y[1]*CtrlFbck.Y[1]+CtrlFbck.Z[1]*CtrlFbck.Z[1]);

	if (CtrlIO.control_mode==2 || CtrlIO.control_mode==3)
	{
		CtrlFbck.engine_speed=fConstrain(CtrlFbck.engine_speed,800,2000);
	}
}


void CONTROL::ControlRC_Check()
{

	xQueuePeek(queueRC_Status,&rc_status_msg,0);					//从队列中获取磁力计数据

	CtrlIO.rc_status = rc_status_msg.rc_status;
	CtrlIO.ever_rc = rc_status_msg.ever_rc;

}



void CONTROL::Integral_Reset()
{

	for (int i=0;i<CTRLPID_NUM;i++)
	{
		CtrlKiTemp.pid_Ki_temp[i]=pid[i].Ki;//暂存积分系数
	}
	/********************起降阶段内环暂停积分***************/
	if (CtrlKiTemp.int_reset==true)
	{
		for (int i=0;i<CTRLPID_NUM;i++)
		{
			pid[i].Ki=0.0f;
			pid[i].integral=0.0f;
		}
		if (CtrlIO.output[3]>(0.8f*OneG))//转速高于悬停转速-100，重置并开始积分
		{
			CtrlKiTemp.int_reset=false;
		}
	}
	else if (CtrlKiTemp.int_reset==false)
	{

		for (int i=0;i<CTRLPID_NUM;i++)
		{
			pid[i].Ki=CtrlKiTemp.pid_Ki_temp[i];
		}
		if (CtrlIO.output[3]<(0.1f*OneG))//转速低于100，停止积分
		{
			CtrlKiTemp.int_reset=true;
		}
		switch(CtrlIO.control_mode)
		{
			case 0://手控，姿态环积分系数置零
			{
				for (u8 i=3;i<12;i++)
				{
					pid[i].Ki=0.0f;
					pid[i].integral=0.0f;
				}
				CtrlLpIO.u1_Tilt[0]=0.0f;
			}
			break;

			case 1://半自控
			{
				for (u8 i=6;i<12;i++)
				{
					pid[i].Ki=0.0f;
					pid[i].integral=0.0f;
				}
				CtrlLpIO.u1_Tilt[0]=0.0f;
			}
			break;
			case 2://自控
			break;
			case 3://自控
			break;
			default:break;
		}
	}
}



void CONTROL::Integral_Restore()
{
	for (int i=0;i<CTRLPID_NUM;i++)
	{
	  pid[i].Ki=CtrlKiTemp.pid_Ki_temp[i];
	}
}




void CONTROL::Out_Loop_XY_Pre()
{
	/***************************************************\
	功能：
		自控模式下外环预处理
		1.遥控器回中对应定点模式
	  2.遥控器有动作对应输入模式（速度输入）
		3.遥控器由输入模式转为定点模式，采用刹车模式衔接，先速度降为0，以当前点为位置输入做定点控制
	\***************************************************/
	double X_err,Y_err;
	bool   X_command_flag,Y_command_flag;
	float  Vel_tol=0.2;
	int    brake_end=50;

	//----------------------检测打杆与否--------------------------------------
	if (CtrlIO.input[1]<10 && CtrlIO.input[1]>-10) X_command_flag = true;
	else  	                                       X_command_flag = false;
	if (CtrlIO.input[0]<10 && CtrlIO.input[0]>-10) Y_command_flag = true;
	else                                           Y_command_flag = false;
	//-----------------------水平速度-----------------------------------------
	if (CtrlLpIO.position_mode==false)
	{
		CtrlLpIO.X_command[1] = -CtrlIO.input[1]*0.0025f;//机头坐标横向速度输入0.0125对应5m/s
		CtrlLpIO.Y_command[1] =  CtrlIO.input[0]*0.0025f;//机头坐标纵向速度输入0.0125对应5m/s
		if (X_command_flag==true && Y_command_flag==true)//小杆量，不打杆
		{
			CtrlLpIO.position_mode = true;
			CtrlLpIO.brake_mode    = true;
			CtrlLpIO.brake_cnt     = 0;
		}
	}
	if (CtrlLpIO.position_mode==true)
	{
		if (X_command_flag==false || Y_command_flag==false)//打杆
		{
			CtrlLpIO.position_mode = false;
		}
		if (CtrlLpIO.brake_mode == true)
		{
			CtrlLpIO.X_command[1] = 0;
			CtrlLpIO.Y_command[1] = 0;
			if (absf(CtrlLpIO.X_command[1]-CtrlFbck.XH[1])<Vel_tol && absf(CtrlLpIO.Y_command[1]-CtrlFbck.YH[1])<Vel_tol)
			{
				CtrlLpIO.brake_cnt++;
			}
			if (CtrlLpIO.brake_cnt==brake_end)
			{
				CtrlLpIO.brake_mode    = false;
				CtrlLpIO.X_command[0]  = CtrlFbck.X[0];//当前位置作为位置给定
				CtrlLpIO.Y_command[0]  = CtrlFbck.Y[0];
				pid[9].integral    = 0.0;
				pid[10].integral   = 0.0;
			}
		}
		else
		{
			//位置误差转到机头坐标误差
			X_err = (CtrlLpIO.X_command[0]-CtrlFbck.X[0])* cos(CtrlFbck.Ang[2]) + (CtrlLpIO.Y_command[0]-CtrlFbck.Y[0])*sin(CtrlFbck.Ang[2]);
			Y_err = (CtrlLpIO.X_command[0]-CtrlFbck.X[0])*-sin(CtrlFbck.Ang[2]) + (CtrlLpIO.Y_command[0]-CtrlFbck.Y[0])*cos(CtrlFbck.Ang[2]);
			CtrlLpIO.X_command[1] = pidX->PID_Controller(X_err,CtrlDt);
			CtrlLpIO.Y_command[1] = pidY->PID_Controller(Y_err,CtrlDt);
		}
	}
}


void CONTROL::Out_Loop_Z_Pre()
{
	double Z_err;
	if (CtrlIO.last_control_mode!=2)  //模式切换瞬态初始化
	{
		CtrlLpIO.Z_command[0] = CtrlFbck.Z[0];
		CtrlLpIO.Z_mid        = CtrlFbck.Z[0];
		//ele->Z_mid = 0.0f;
		CtrlLpIO.Z_mid_pwm    = CtrlIO.input[3];
	}
	CtrlLpIO.Z_command[0] = CtrlLpIO.Z_mid - (CtrlIO.input[3]-CtrlLpIO.Z_mid_pwm)*0.005f;
	Z_err             = CtrlLpIO.Z_command[0]-CtrlFbck.Z[0];
	CtrlLpIO.Z_command[1] = pidZ->PID_Controller(Z_err,CtrlDt);
	//ele->Z_command[1]= ele->Z_mid - (CtrlIO->input[3]-ele->Z_mid_pwm)*0.005f;

}



void CONTROL::Pos_Loop_Step()
{
//	double X_err,Y_err,Z_err;
//	float sinY,cosY;
//	arm_sin_cos_f32(ahrs[1].Ang[2]*R2D,&sinY,&cosY);
//	double Vx,Vy;
//
//	ele->X_command[0] = trajectory.cur_target_pos_ned[0];
//	ele->Y_command[0] = trajectory.cur_target_pos_ned[1];
////	ele->Z_command[0] = trajectory.cur_target_pos_ned[2];
//	ele->Z_command[0] = ele->Z_mid - (CtrlIO->input[3]-ele->Z_mid_pwm)*0.005f;
//
//	X_err = ele->X_command[0] - CtrlFbck->X[0];
//	Y_err = ele->Y_command[0] - CtrlFbck->Y[0];
//	Z_err = ele->Z_command[0] - CtrlFbck->Z[0];
//
//	Vx = Pid_Controller(pidX,X_err);
//	Vy = Pid_Controller(pidY,Y_err);
//
//	ele->X_command[1] =  Vx*cosY + Vy*sinY;
//	ele->Y_command[1] = -Vx*sinY + Vy*cosY;
//	ele->Z_command[1] = Pid_Controller(pidZ,Z_err);
//
}


void CONTROL::Out_Loop_Step()
{
	double Vel_err[3];
	//---------------------速度输入限幅----------------------//

	CtrlLpIO.X_command[1] = fConstrain(CtrlLpIO.X_command[1],-5,5);
	CtrlLpIO.Y_command[1] = fConstrain(CtrlLpIO.Y_command[1],-5,5);
	CtrlLpIO.Z_command[1] = fConstrain(CtrlLpIO.Z_command[1],-5,5);

	//-----------------------速度误差------------------------//

	Vel_err[0] = CtrlLpIO.X_command[1]-CtrlFbck.XH[1];  //X_command[1]机头速度
	Vel_err[1] = CtrlLpIO.Y_command[1]-CtrlFbck.YH[1];
	Vel_err[2] = CtrlLpIO.Z_command[1]-CtrlFbck.Z[1];

	//---------------------加速度指令，NED-------------------//

	CtrlLpIO.acc_command[0] = pidXRate->PID_Controller(Vel_err[0],CtrlDt);//加速度指令,NED
	CtrlLpIO.acc_command[1] = pidYRate->PID_Controller(Vel_err[1],CtrlDt);
	CtrlLpIO.acc_command[2] = pidZRate->PID_Controller(Vel_err[2],CtrlDt);

}


void CONTROL::Underactuated_Loop_Step()
{

	xQueuePeek(queueAccDatFil, &acc_fil, 0);					//从队列中获取高度数据

	//-----------------------加速度限幅----------------------//
	CtrlLpIO.acc_command[0] = fConstrain(CtrlLpIO.acc_command[0],-3.5,3.5);
	CtrlLpIO.acc_command[1] = fConstrain(CtrlLpIO.acc_command[1],-3.5,3.5);
	CtrlLpIO.acc_command[2] = fConstrain(CtrlLpIO.acc_command[2],-OneG,OneG);

	float ad = CtrlLpIO.acc_command[0];
	float bd = CtrlLpIO.acc_command[1];
	float cd = CtrlLpIO.acc_command[2] - OneG;
	float dd,d1,d2;
	float u1_Tilt_D;
//	float ax = -ICM20602->AccFil_3nd[0];
//	float ay = -ICM20602->AccFil_3nd[1];
//	float az = -ICM20602->AccFil_3nd[2];
//	float ax = ahrs[0].Acc_Fil_buffer[0][ahrs[0].acc_buffer_cnt];
//	float ay = ahrs[0].Acc_Fil_buffer[1][ahrs[0].acc_buffer_cnt];
//	float az = ahrs[0].Acc_Fil_buffer[2][ahrs[0].acc_buffer_cnt];
	float ax = -acc_fil.acc_filter[0] ;//- gps->acc_bias_EKF[0];
	float ay = -acc_fil.acc_filter[1] ;//- gps->acc_bias_EKF[1];
	float az = -acc_fil.acc_filter[2] ;//- gps->acc_bias_EKF[2];
	float eta1;
	float cosPd,sinPd;

	//---------------------标称系统--------------------//
	/***************************************************\
	由期望的NED加速度解算出期望的角度和拉力
	\***************************************************/
	//nominal thrust
	CtrlLpIO.u1_Bar[0] = sqrt( fConstrain((ad*ad + bd*bd + cd*cd - ax*ax - ay*ay),0,400) );
	//nominal pitch
	eta1           = atan2(ad,-cd);
	CtrlLpIO.Pd_Bar    = asin(fConstrain(ax/(sqrt(ad*ad+cd*cd)),-1,1))-eta1;
	//nominal roll
	cosPd          = cos(CtrlLpIO.Pd_Bar);
	sinPd          = sin(CtrlLpIO.Pd_Bar);
	dd             = ad*sinPd + cd*cosPd;
	d1             = bd*CtrlLpIO.u1_Bar[0] + dd*ay;
	d2             = bd*ay - dd*CtrlLpIO.u1_Bar[0];
	CtrlLpIO.Rd_Bar    = atan2(d1,d2);
	//--------------------拉力补偿---------------------//
	u1_Tilt_D = CtrlLpIO.ko1*(az + CtrlLpIO.u1_Bar[0]);
	CtrlLpIO.u1_Tilt[0] = CtrlLpIO.u1_Tilt[0] + u1_Tilt_D*CtrlDt;
	//-------------------------------------------------//

	CtrlLpIO.thrust_command[0] = CtrlLpIO.u1_Bar[0] + CtrlLpIO.u1_Tilt[0];
	CtrlLpIO.roll_command[0]  = CtrlLpIO.Rd_Bar;
	CtrlLpIO.pitch_command[0]  = CtrlLpIO.Pd_Bar;
	CtrlLpIO.thrust_command[0] = fConstrain(CtrlLpIO.thrust_command[0],6,20);
}

void CONTROL::In_Loop_Step()
{
	//----------------滚转俯仰角输入限幅----------------//
	CtrlLpIO.roll_command[0] = fConstrain(CtrlLpIO.roll_command[0],-CtrlLpIO.AngleLimitR,CtrlLpIO.AngleLimitR);
	CtrlLpIO.pitch_command[0] = fConstrain(CtrlLpIO.pitch_command[0],-CtrlLpIO.AngleLimitP,CtrlLpIO.AngleLimitP);
	if (CtrlIO.last_yaw_mode==0 && CtrlIO.yaw_mode==1 )
	{
		CtrlLpIO.yaw_mid = CtrlFbck.Ang[2];
	}
	if (CtrlIO.last_fly_mode==1 && CtrlIO.fly_mode==0 )
	{
		CtrlLpIO.yaw_mid = CtrlFbck.Ang[2];
	}
	//----------------滚转俯仰角变化率输入----------------
	CtrlLpIO.roll_err         = CtrlLpIO.roll_command[0]-CtrlFbck.Ang[0];
	CtrlLpIO.pitch_err        = CtrlLpIO.pitch_command[0]-CtrlFbck.Ang[1];
	CtrlLpIO.roll_command[1]  = pidRol->PID_Controller(CtrlLpIO.roll_err,CtrlDt);
	CtrlLpIO.pitch_command[1] = pidPit->PID_Controller(CtrlLpIO.pitch_err,CtrlDt);
	//--------------------偏航控制--------------------
	if (CtrlIO.yaw_mode==1 && CtrlIO.fly_mode==0)//磁力计正常更新才允许锁尾
	{
		CtrlLpIO.yaw_command[0] = CtrlLpIO.yaw_mid + CtrlIO.input[2]*0.18f*D2R;
	}
	if (CtrlIO.fly_mode==1)
	{
//		ele->yaw_command[0] = trajectory.yaw_command;
	}
	CtrlLpIO.yaw_err = (CtrlLpIO.yaw_command[0]-CtrlFbck.Ang[2]);
	//----------------消除-180°到+180°的跳变----------------
	if      (CtrlLpIO.yaw_err<-PI)  CtrlLpIO.yaw_err+=2*PI;
	else if (CtrlLpIO.yaw_err>PI)   CtrlLpIO.yaw_err-=2*PI;
	//------------------------------------------------------
	CtrlLpIO.yaw_command[1] = pidYaw->PID_Controller(CtrlLpIO.yaw_err,CtrlDt);


	//----------------欧拉角变化率到pqr----------------
	CtrlLpIO.pqr_command[0]=CtrlLpIO.roll_command[1]-CtrlLpIO.yaw_command[1]*sin(CtrlFbck.Ang[1]);
	CtrlLpIO.pqr_command[1]=CtrlLpIO.pitch_command[1]*cos(CtrlFbck.Ang[0])+CtrlLpIO.yaw_command[1]*cos(CtrlFbck.Ang[1])*sin(CtrlFbck.Ang[0]);
	CtrlLpIO.pqr_command[2]=-CtrlLpIO.pitch_command[1]*sin(CtrlFbck.Ang[0])+CtrlLpIO.yaw_command[1]*cos(CtrlFbck.Ang[1])*cos(CtrlFbck.Ang[0]);
	if (CtrlIO.yaw_mode==0 && CtrlIO.fly_mode==0)
	{
		CtrlLpIO.pqr_command[2] = CtrlIO.input[2]*0.18f*D2R;//不锁尾，遥控器输入为航向角速度指令
	}
}



void CONTROL::Gyro_Control()
{
	CtrlIO.gyro_output[0] = -CtrlIO.gyro_gain*CtrlFbck.pqr[1]/fConstrain(CtrlFbck.engine_speed,800.0f,2000.0f);
	CtrlIO.gyro_output[1] = +CtrlIO.gyro_gain*CtrlFbck.pqr[0]/fConstrain(CtrlFbck.engine_speed,800.0f,2000.0f);
	CtrlIO.gyro_output[2] = 0.0f;
}



void CONTROL::Control_Output()
{
	for (u8 i=0;i<3;i++)
	{
		CtrlIO.pqr_err[i]=CtrlLpIO.pqr_command[i]-CtrlFbck.pqr[i];
	}
	if (CtrlIO.last_control_mode==2 || CtrlIO.last_control_mode==3)
	{
		for (u8 i=0;i<3;i++)
		{
			pid[i].integral=0;//由自控切回半自控重置内环积分
		}
	}
	CtrlIO.output_pid[0]=pidRolRate->PID_Controller(CtrlIO.pqr_err[0],CtrlDt);
	CtrlIO.output_pid[1]=pidPitRate->PID_Controller(CtrlIO.pqr_err[1],CtrlDt);
	CtrlIO.output_pid[2]=pidYawRate->PID_Controller(CtrlIO.pqr_err[2],CtrlDt);
	CtrlIO.output1[0]=pid[0].iout;
	CtrlIO.output1[1]=pid[1].iout;
	CtrlIO.output1[2]=pid[2].iout;
	//------------------------三轴力距输出-----------------------------
	CtrlIO.output[0]=(CtrlIO.mid_trim[0]/((double)RAD_TO_PWM))+CtrlIO.output_pid[0];
	CtrlIO.output[1]=(CtrlIO.mid_trim[1]/((double)RAD_TO_PWM))+CtrlIO.output_pid[1];
	CtrlIO.output[2]=(CtrlIO.mid_trim[2]/((double)RAD_TO_PWM))+CtrlIO.output_pid[2];
	CtrlIO.output2[0]=CtrlIO.output[0]-CtrlIO.output1[0];
	CtrlIO.output2[1]=CtrlIO.output[1]-CtrlIO.output1[1];
	CtrlIO.output2[2]=CtrlIO.output[2]-CtrlIO.output1[2];
	//------------------------拉力输出-----------------------------------------------
	CtrlIO.output[3] = CtrlLpIO.thrust_command[0];
}


void CONTROL::Control_Output2()
{
	for (u8 i=0;i<3;i++)
	{
		CtrlIO.pqr_err[i]=CtrlLpIO.pqr_command[i]-CtrlFbck.pqr[i];
	}
	if (CtrlIO.last_control_mode!=2 && CtrlIO.last_control_mode!=3)
	{
		MRAC_Init2();//模式切换瞬态初始化
	}
	MRAC_Calc2(CtrlIO.pqr_err);
	CtrlIO.output_pid[0]=CtrlMRAC2.uc2[0];
	CtrlIO.output_pid[1]=CtrlMRAC2.uc2[1];
	CtrlIO.output_pid[2]=CtrlMRAC2.uc2[2];
	CtrlIO.output1[0]=CtrlMRAC2.uc2_Tilt[0];
	CtrlIO.output1[1]=CtrlMRAC2.uc2_Tilt[1];
	CtrlIO.output1[2]=CtrlMRAC2.uc2_Tilt[2];

//------------------------三轴力距输出-----------------------------
	CtrlIO.output[0]=(CtrlIO.mid_trim[0]/((double)RAD_TO_PWM))+CtrlIO.output_pid[0];
	CtrlIO.output[1]=(CtrlIO.mid_trim[1]/((double)RAD_TO_PWM))+CtrlIO.output_pid[1];
	CtrlIO.output[2]=(CtrlIO.mid_trim[2]/((double)RAD_TO_PWM))+CtrlIO.output_pid[2];
	CtrlIO.output2[0]=CtrlIO.output[0]-CtrlIO.output1[0];
	CtrlIO.output2[1]=CtrlIO.output[1]-CtrlIO.output1[1];
	CtrlIO.output2[2]=CtrlIO.output[2]-CtrlIO.output1[2];
//------------------------拉力输出-----------------------------------------------
	CtrlIO.output[3] = CtrlLpIO.thrust_command[0];


}



void CONTROL::Output_To_Motor()
{
	//--------------------舵机输出----------------------------------
	control.two_dir_alloc_mch(CtrlIO.output1, CtrlIO.output2, dir.p_limits, dir.u);

	CtrlIO.cs_output[0] = (int) ((dir.u[0]) * RAD_TO_PWM);
	CtrlIO.cs_output[1] = (int) ((dir.u[1]) * RAD_TO_PWM);
	CtrlIO.cs_output[2] = (int) ((dir.u[2]) * RAD_TO_PWM);
	CtrlIO.cs_output[3] = (int) ((dir.u[3]) * RAD_TO_PWM);
	//--------------------电机输出----------------------------------
	double K_pwm = OneG/((HOVER_PWM-1000)*(HOVER_PWM-1000));
	if(CtrlIO.rc_status == NORMAL)
	{
		CtrlIO.mt_output[0] = (int)(sqrt((CtrlIO.output[3])/K_pwm))+1000;
	}
	else if(CtrlIO.rc_status==CLOSE)
	{
		CtrlIO.mt_output[0] = 1000;
	}
	else
	{
		CtrlIO.mt_output[0] = DRIFT_PWM;
	}

	control_output.mt_output[0] = CtrlIO.mt_output[0];
	control_output.cs_output[0] = CtrlIO.cs_output[0];
	control_output.cs_output[1] = CtrlIO.cs_output[1];
	control_output.cs_output[2] = CtrlIO.cs_output[2];
	control_output.cs_output[3] = CtrlIO.cs_output[3];
	xQueueOverwrite(queueControlOutputDat,&control_output);
}



void CONTROL::MRAC_Init2()
{
	CtrlMRAC2.k1          = 100.0f;
	CtrlMRAC2.k2[0][0]    = 400.0f;
	CtrlMRAC2.k2[1][0]    = 10.0f;
	CtrlMRAC2.k2[2][0]    = 10.0f;
	CtrlMRAC2.k2[0][1]    = 400.0f;
	CtrlMRAC2.k2[1][1]    = 10.0f;
	CtrlMRAC2.k2[2][1]    = 10.0f;
	CtrlMRAC2.k2[0][2]    = 400.0f;
	CtrlMRAC2.k2[1][2]    = 0.0f;
	CtrlMRAC2.k2[2][2]    = 0.0f;
//	ele->k5[0][0]    = 0.0f;
	CtrlMRAC2.k5[0][0]    = 1.0f;
	CtrlMRAC2.k5[0][1]    = 0.0001;
	CtrlMRAC2.k5[0][2]    = 0.0f;
	CtrlMRAC2.k5[0][3]    = 0.0f;
//	ele->k5[1][0]    = 0.0f;
	CtrlMRAC2.k5[1][0]    = 1.0f;
	CtrlMRAC2.k5[1][1]    = 0.0001;
	CtrlMRAC2.k5[1][2]    = 0.0f;
	CtrlMRAC2.k5[1][3]    = 0.0f;
//	ele->k5[2][0]    = 0.0f;
	CtrlMRAC2.k5[2][0]    = 1.0f;
	CtrlMRAC2.k5[2][1]    = 0.000056;
	CtrlMRAC2.k5[2][2]    = 0.0f;
	CtrlMRAC2.k5[2][3]    = 0.0f;
	CtrlMRAC2.kuc2[0]     = 0.1f;
	CtrlMRAC2.kuc2[1]     = 0.1f;
	CtrlMRAC2.kuc2[2]     = 0.1f;
	CtrlMRAC2.w3[0][0]    = 0.0f;
	CtrlMRAC2.w3[1][0]    = 0.0f;
	CtrlMRAC2.w3[2][0]    = 0.0f;
	CtrlMRAC2.w3[0][1]    = 0.0f;
	CtrlMRAC2.w3[1][1]    = 0.0f;
	CtrlMRAC2.w3[2][1]    = 0.0f;
	CtrlMRAC2.w3[0][2]    = 0.0f;
	CtrlMRAC2.w3[1][2]    = 0.0f;
	CtrlMRAC2.w3[2][2]    = 0.0f;
//	ele->w4[0]       = -1.0f;
	CtrlMRAC2.w4[0]       = -5.0f;
	CtrlMRAC2.w4[1]       = 0.01006;
	CtrlMRAC2.w4[2]       = -0.53;
	CtrlMRAC2.w4[3]       = 0.0001406731;
	CtrlMRAC2.uc2_Tilt[0] = 0.0f;
	CtrlMRAC2.uc2_Tilt[1] = 0.0f;
	CtrlMRAC2.uc2_Tilt[2] = CtrlIO.mid_trim[2]/((double)RAD_TO_PWM);
	CtrlMRAC2.uc2[0]      = 0.0f;
	CtrlMRAC2.uc2[1]      = 0.0f;
	CtrlMRAC2.uc2[2]      = CtrlMRAC2.uc2_Tilt[2];
	CtrlMRAC2.Ctrl_Flag[0]= 1;
	CtrlMRAC2.Ctrl_Flag[1]= 1;
	CtrlMRAC2.Ctrl_Flag[2]= 1;
}




void CONTROL::MRAC_Calc2(double pqr_input[3])
{

	float DM[3];
	float g4_Bar;
	float c_m;
	double pqr_Pred_D[3];
	double w3_D[3][3];
	double w4_D[4];
	double k5tw4_D[3][4];
	double uc2_Tilt_D[3];
//	float pm[3];
	//reference model
	DM[0]  = DM1_MRAC;
	DM[1]  = DM2_MRAC;
	DM[2]  = DM3_MRAC;
	g4_Bar = G4_BAR;
	c_m    = 20*D2R;
	//phi3
	CtrlMRAC2.phi3[0][0] = 1.0f;
	CtrlMRAC2.phi3[1][0] = CtrlFbck.YH[1];
	CtrlMRAC2.phi3[2][0] = CtrlFbck.Z[1];
	CtrlMRAC2.phi3[0][1] = 1.0f;
	CtrlMRAC2.phi3[1][1] = CtrlFbck.XH[1];
	CtrlMRAC2.phi3[2][1] = CtrlFbck.Z[1];
	CtrlMRAC2.phi3[0][2] = 1.0f;
	CtrlMRAC2.phi3[1][2] = CtrlFbck.engine_speed;
	CtrlMRAC2.phi3[2][2] = 0;
	//phi4
//	ele->phi4[0] = g4_Bar;
//	ele->phi4[1] = -CtrlFbck->engine_speed*CtrlFbck->W[0];
//	ele->phi4[2] = CtrlFbck->W[0]*CtrlFbck->W[0];
//	ele->phi4[3] = CtrlFbck->engine_speed*CtrlFbck->engine_speed;
	CtrlMRAC2.phi4[0] = CtrlFbck.W[0]*sqrt(CtrlLpIO.u1_Bar[0]);
	//auxilary system
	for (u8 i=0;i<3;i++)
	{
		CtrlMRAC2.uc2_Bar[i] = fConstrain(pqr_input[i]*pid[i].Kp,-c_m,c_m);
//		pqr_Pred_D[i]   =   (ele->w3[0][i]*ele->phi3[0][i] + ele->w3[1][i]*ele->phi3[1][i] + ele->w3[2][i]*ele->phi3[2][i])
//		                  + g4_Bar*DM[i]*ele->uc2[i]
//		                  + (ele->w4[0]*ele->phi4[0] + ele->w4[1]*ele->phi4[1] + ele->w4[2]*ele->phi4[2] +ele->w4[3]*ele->phi4[3])*DM[i]*ele->uc2[i]
//		                  - ele->k1*(ele->pqr_Pred[i]-CtrlFbck->pqr[i]);
		pqr_Pred_D[i]   =   (CtrlMRAC2.w3[0][i]*CtrlMRAC2.phi3[0][i] + CtrlMRAC2.w3[1][i]*CtrlMRAC2.phi3[1][i] + CtrlMRAC2.w3[2][i]*CtrlMRAC2.phi3[2][i])
		                  + g4_Bar*DM[i]*CtrlMRAC2.uc2[i]
		                  + (CtrlMRAC2.w4[0]*CtrlMRAC2.phi4[0])*DM[i]*CtrlMRAC2.uc2[i]
		                  - CtrlMRAC2.k1*(CtrlMRAC2.pqr_Pred[i]-CtrlFbck.pqr[i]);
		for (u8 j=0;j<3;j++)
		{
			w3_D[j][i]    = -CtrlMRAC2.k2[j][i]*CtrlMRAC2.phi3[j][i]*(CtrlMRAC2.pqr_Pred[i]-CtrlFbck.pqr[i]);
			CtrlMRAC2.w3[j][i] =  CtrlMRAC2.w3[j][i] + w3_D[j][i]*CtrlDt;
		}
//		for (u8 k=0;k<4;k++)
//		{
//			k5tw4_D[i][k] = -ele->k5[i][k]*ele->phi4[k]*DM[i]*ele->uc2[i]*(ele->pqr_Pred[i]-CtrlFbck->pqr[i]);
//		}
		k5tw4_D[i][0] = -CtrlMRAC2.k5[i][0]*CtrlMRAC2.phi4[0]*DM[i]*CtrlMRAC2.uc2[i]*(CtrlMRAC2.pqr_Pred[i]-CtrlFbck.pqr[i]);
		CtrlMRAC2.pqr_Pred[i] = CtrlMRAC2.pqr_Pred[i] + pqr_Pred_D[i]*CtrlDt;
	}
//	for (u8 i=0;i<4;i++)
//	{
//		w4_D[i]    = k5tw4_D[0][i] + k5tw4_D[1][i] + k5tw4_D[2][i];
//		ele->w4[i] = ele->w4[i] + w4_D[i]*CtrlDt;
//	}
	w4_D[0]    = k5tw4_D[0][0] + k5tw4_D[1][0] + k5tw4_D[2][0];
	CtrlMRAC2.w4[0] = CtrlMRAC2.w4[0] + w4_D[0]*CtrlDt;
	//control output
	for (u8 i=0;i<3;i++)
	{
//		uc2_Tilt_D[i]    = -ele->kuc2[i]*
//										   ((ele->w3[0][i]*ele->phi3[0][i] + ele->w3[1][i]*ele->phi3[1][i] + ele->w3[2][i]*ele->phi3[2][i])
//										   +((g4_Bar + ele->w4[0]*ele->phi4[0] + ele->w4[1]*ele->phi4[1] + ele->w4[2]*ele->phi4[2] +ele->w4[3]*ele->phi4[3])*DM[i]*ele->uc2_Tilt[i])
//										   +((ele->w4[0]*ele->phi4[0] + ele->w4[1]*ele->phi4[1] + ele->w4[2]*ele->phi4[2] +ele->w4[3]*ele->phi4[3])*DM[i]*ele->uc2_Bar[i]));
		uc2_Tilt_D[i]    = -CtrlMRAC2.kuc2[i]*
										   ((CtrlMRAC2.w3[0][i]*CtrlMRAC2.phi3[0][i] + CtrlMRAC2.w3[1][i]*CtrlMRAC2.phi3[1][i] + CtrlMRAC2.w3[2][i]*CtrlMRAC2.phi3[2][i])
										   +((g4_Bar + CtrlMRAC2.w4[0]*CtrlMRAC2.phi4[0])*DM[i]*CtrlMRAC2.uc2_Tilt[i])
										   +((CtrlMRAC2.w4[0]*CtrlMRAC2.phi4[0])*DM[i]*CtrlMRAC2.uc2_Bar[i]));
		CtrlMRAC2.uc2_Tilt[i] = CtrlMRAC2.uc2_Tilt[i] + uc2_Tilt_D[i]*CtrlDt;
		CtrlMRAC2.uc2[i]      = CtrlMRAC2.uc2_Bar[i] + CtrlMRAC2.uc2_Tilt[i];
		CtrlMRAC2.uc2[i]      = fConstrain(CtrlMRAC2.uc2[i],-c_m,c_m);
//		ele->g3_uc2[0]   = (ele->w3[0][i]*ele->phi3[0][i] + ele->w3[1][i]*ele->phi3[1][i] + ele->w3[2][i]*ele->phi3[2][i])/((g4_Bar + ele->w4[0]*ele->phi4[0] + ele->w4[1]*ele->phi4[1] + ele->w4[2]*ele->phi4[2] +ele->w4[3]*ele->phi4[3])*DM[i]);
		CtrlMRAC2.g3_uc2[0]   = (CtrlMRAC2.w3[0][i]*CtrlMRAC2.phi3[0][i] + CtrlMRAC2.w3[1][i]*CtrlMRAC2.phi3[1][i] + CtrlMRAC2.w3[2][i]*CtrlMRAC2.phi3[2][i])/((g4_Bar + CtrlMRAC2.w4[0]*CtrlMRAC2.phi4[0])*DM[i]);
		if (absf(CtrlMRAC2.uc2_Tilt[i])>=(0.5f*c_m))
		{
			CtrlMRAC2.Ctrl_Flag[i]=0;
		}
		else
		{
			CtrlMRAC2.Ctrl_Flag[i]=1;
		}
	}
}



void CONTROL::Transition_Init()
{
			CtrlLpIO.Phase        = 0;
			CtrlLpIO.Vydd         = 0.0;
			CtrlLpIO.X_command[0] = CtrlFbck.X[0];
			CtrlLpIO.Y_command[0] = CtrlFbck.Y[0];
			CtrlLpIO.Y_command[1] = 0.0;
			CtrlLpIO.Tran_cnt = 0;
//		if (ele->Phase!=5)//非定点阶段
//		{
////			  X_err             = (ele->X_command[0]-CtrlFbck->X[0])* cos(CtrlFbck->Ang[2])+(ele->Y_command[0]-CtrlFbck->Y[0])*sin(CtrlFbck->Ang[2]);
////				ele->X_command[1] = 0.0f;
//			ele->Z_command[1] = 2.0;
//		}

}

void CONTROL::Transition_Calc()
{
	double evy;
	double X_err,Y_err;
	evy    = CtrlLpIO.Y_command[1]-CtrlFbck.YH[1];
	float VRate = 2.0f;
	float VCase2 = 4.0f;
	switch (CtrlLpIO.Phase)
	{
		case 0://加速阶段
			//if (CtrlFbck->Ang[0] >= (10*D2R))//倾转至设定角度，进入下一阶段
			if (absf(CtrlFbck.YH[1])>=(CtrlLpIO.VelSet-0.5f) || CtrlMRAC2.uc2_Tilt[0]>=(15*D2R))
			{
			  CtrlLpIO.Phase = 1;
			}
			CtrlLpIO.AngleLimitR = 80*D2R;
			CtrlLpIO.AngleLimitP = 20*D2R;
			CtrlLpIO.Vydd         = VRate;
			CtrlLpIO.Y_command[1] = CtrlLpIO.Y_command[1]+CtrlLpIO.Vydd*CtrlDt;
			CtrlLpIO.Y_command[1] = fConstrain(CtrlLpIO.Y_command[1],-CtrlLpIO.VelSet,CtrlLpIO.VelSet);
			//		  ele->AngleLimit   = 20*D2R;
			//		  ele->Y_command[1] = 2.0f;
			break;
		case 1://减速
			if (CtrlMRAC2.uc2_Tilt[0]<=(-15*D2R))
			{
				CtrlLpIO.Phase = 2;
			}
			if (absf(CtrlFbck.YH[1])<=(VCase2+0.5f))//速度稳定，进入下一阶段
			{
				CtrlLpIO.Phase = 4;
				CtrlLpIO.Tran_cnt=0;
				CtrlLpIO.Z_mid = CtrlFbck.Z[0];
				pid[11].integral=0.0f;
			}
			CtrlLpIO.AngleLimitR = 80*D2R;
			CtrlLpIO.AngleLimitP = 20*D2R;
			CtrlLpIO.Vydd         = -VRate;
			CtrlLpIO.Y_command[1] = CtrlLpIO.Y_command[1]+CtrlLpIO.Vydd*CtrlDt;
			CtrlLpIO.Y_command[1] = fConstrain(CtrlLpIO.Y_command[1],-CtrlLpIO.VelSet,CtrlLpIO.VelSet);
//			ele->Y_command[1]=0.0f;
			break;
		case 2://爬高减速
			if (absf(CtrlFbck.YH[1])<=(VCase2+0.5f))//速度稳定，进入下一阶段
			{
				CtrlLpIO.Phase = 3;
				CtrlLpIO.Tran_cnt=0;
			}
			CtrlLpIO.AngleLimitR = 80*D2R;
			CtrlLpIO.AngleLimitP = 20*D2R;
			CtrlLpIO.Vydd         = -VRate;
			CtrlLpIO.Y_command[1] = CtrlLpIO.Y_command[1]+CtrlLpIO.Vydd*CtrlDt;
			CtrlLpIO.Y_command[1] = fConstrain(CtrlLpIO.Y_command[1],-CtrlLpIO.VelSet,CtrlLpIO.VelSet);
//			ele->Y_command[1]=0.0f;
			CtrlLpIO.Z_command[1] = -0.4f-0.2f*absf(CtrlLpIO.roll_command[0]-CtrlFbck.Ang[0])*R2D;
			break;
		case 3://进入悬停（减加速度）
			if (absf(CtrlLpIO.Z_command[1]-CtrlFbck.Z[1])<0.4f)//加速度减至0，进入下一阶段
			{
				CtrlLpIO.Tran_cnt++;
			}
			if (CtrlLpIO.Tran_cnt>=100 )//速度减至0，进入下一阶段
			{
				CtrlLpIO.Phase = 4;
				CtrlLpIO.Tran_cnt=0;
				CtrlLpIO.Z_mid = CtrlFbck.Z[0];
				pid[11].integral=0.0f;
			}
			CtrlLpIO.AngleLimitR = 20*D2R;
			CtrlLpIO.AngleLimitP = 20*D2R;
			CtrlLpIO.Vydd         = -VRate;
			CtrlLpIO.Y_command[1] = CtrlLpIO.Y_command[1]+CtrlLpIO.Vydd*CtrlDt;
//			ele->Y_command[1]=0.0f;
			CtrlLpIO.Z_command[1]= 0.0f;
			if (Sign(CtrlLpIO.Y_command[1]) != Sign(VRate))
			{
				CtrlLpIO.Y_command[1]=0.0f;
			}
			break;
		case 4://进入悬停（减速度）
			if (absf(evy)<=0.2f && CtrlLpIO.Y_command[1]==0.0f)
			{
				CtrlLpIO.Tran_cnt++;
			}
			if (CtrlLpIO.Tran_cnt>=100 )//速度减至0，进入下一阶段
			{
				CtrlLpIO.Phase = 5;
				CtrlLpIO.X_command[0]  = CtrlFbck.X[0];//当前位置作为位置给定
				CtrlLpIO.Y_command[0]  = CtrlFbck.Y[0];
				pid[9].integral    = 0.0f;
				pid[10].integral   = 0.0f;
			}
			CtrlLpIO.AngleLimitR = 20*D2R;
			CtrlLpIO.AngleLimitP = 20*D2R;
			CtrlLpIO.Vydd         = -VRate;
			CtrlLpIO.Y_command[1] = CtrlLpIO.Y_command[1]+CtrlLpIO.Vydd*CtrlDt;
			if (Sign(CtrlLpIO.Y_command[1]) != Sign(VRate))
			{
				CtrlLpIO.Y_command[1]=0.0f;
			}
			break;
		case 5://定点悬停
			CtrlLpIO.AngleLimitR = 20*D2R;
			CtrlLpIO.AngleLimitP = 20*D2R;
			X_err             = (CtrlLpIO.X_command[0]-CtrlFbck.X[0])* cos(CtrlFbck.Ang[2])+(CtrlLpIO.Y_command[0]-CtrlFbck.Y[0])*sin(CtrlFbck.Ang[2]);
			Y_err             = (CtrlLpIO.X_command[0]-CtrlFbck.X[0])*-sin(CtrlFbck.Ang[2])+(CtrlLpIO.Y_command[0]-CtrlFbck.Y[0])*cos(CtrlFbck.Ang[2]);
			CtrlLpIO.X_command[1] = pidX->PID_Controller(X_err,CtrlDt);  //机头坐标速度给定
			CtrlLpIO.Y_command[1] = pidY->PID_Controller(Y_err,CtrlDt);
		  break;
		default:break;
	}
}


